/*
 * @Description: 定位线程
 * @Author: Dryad
 * @Date: 2019-07-03 10:07:20
 */
#include <rtthread.h>
#include <rthw.h>
#include "./Location.h"
#include "../Drivers/ARES-8015-E-AC.hpp"
#include "../Drivers/Mecanum_AGV.hpp"
#include "../Drivers/TL740.h"
#include "../Drivers/PGV.h"
#include "../HALayer/stm32_tim.hpp"
#include "../Math/Kalman_Filter_Velocity.hpp"
#include "../Math/Kalman_Filter_Coor.hpp"
#include "../Math/My_Math.hpp"
#include "../parameter.h"
#include "./Modbus/modbus_slave_core.h"
#include "./Move_Control.h"

#define LOG_TAG "location"  /* 该模块TAG为location */
#define LOG_LVL LOG_LVL_DBG /* 静态过滤级别为调试级 */
#include <ulog.h>           /* 必须放在宏定义下面 */

static void Location_Thread(void *parameter); /* AGV定位线程 */
static struct rt_thread location_thread;      /* AGV定位线程句柄 */
ALIGN(RT_ALIGN_SIZE)                          /* 线程栈4字节对齐 */
static char location_thread_stack[1024];      /* 线程栈起始地址 */

static void Wait_PGV_Thread(void *parameter);   /* 等待PGV初始化完成线程 */
static void Wait_TL740_Thread(void *parameter); /* 等待陀螺仪初始化完成线程 */
static void Send_PGV_Read_CMD(void *parameter); /* 定时读取PGV数据 */

static struct PGV_Response_Data pgv_data;
static float z_rate = 0.0f, z_heading = 0.0f; /* 陀螺仪角速度，角度，上一时刻角度 */

static TIM_Base_Class encoder_tim(TIM7); /* 用于测量控制周期的定时器 */
static float delta_time_s = 0.0f;        /* 两次定位间隔时间(s) */

static union Kalman_Filter_Velocity_Namespace::Input_Control_Union agv_motor_velocity; /* 读取的电机速度 */
static union Kalman_Filter_Velocity_Namespace::Sys_State_Union agv_velocoity;          /* AGV的速度 */
static union Kalman_Filter_Coor_Namespace::Sys_State_Union agv_coor, pgv_measure;      /* AGV的坐标，PGV测量得到的坐标 */
static arm_matrix_instance_f32 agv_velocity_covariance;                                /* AGV的速度协方差矩阵 */

static struct Location_Coor_Struct location_coor;
static struct Location_Velocity_Struct location_velocity;

namespace Sensor_Init_Namespace
{
enum
{
    PGV_Init_Event,     /* PGV初始化完成事件 */
    TL740_Init_Event,   /* TL740初始化完成事件 */
    Received_PGV_Event, /* 接收到了PGV数据事件 */
};
}
static rt_event_t sensor_event;                 /* 传感器初始化事件 */
static struct rt_timer send_pgv_read_cmd_timer; /* 定时读取PGV数据 */

static bool stop_by_first_read_data_matrix = false; /* true表示第一次读取到二维码，车辆停止 */

void Location_Thread_Init(void)
{
    rt_err_t rt_result;
    PGV_Thread_Init(agv_parameter->pgv_x, agv_parameter->pgv_y,
                    (agv_parameter->pgv_theta + agv_parameter->pgv_theta_offset));              /* 初始化PGV线程 */
    TL740_Thread_Init();                                                                        /* 初始化陀螺仪线程 */
    Kalman_Filter_Velocity_Namespace::Init(1.46806f, 1.8765e-02f,                               /* 电机执行噪声，陀螺仪测量噪声 */
                                           agv_parameter->wheel_diameter / 2.0f,                /* 车轮半径 */
                                           agv_parameter->distance_x, agv_parameter->distance_y /* 车身尺寸 */
    );                                                                                          /* 噪声测量完成，有关角度的噪声单位均为rad */
    Kalman_Filter_Coor_Namespace::Init(0.121733f, 0.121733f, 2.0795e-05f,
                                       7.70475f, 0.32205f); /* 噪声测量完成，有关角度的噪声单位均为rad */

    location_coor.x_coor = 0.0f;
    location_coor.y_coor = 0.0f;
    location_coor.theta = 0.0f;
    location_velocity.vx = 0.0f;
    location_velocity.vy = 0.0f;
    location_velocity.w = 0.0f;

    sensor_event = rt_event_create("sensor event", RT_IPC_FLAG_FIFO); /* 定位所需传感器初始化事件 */
    rt_timer_init(&send_pgv_read_cmd_timer, "send pgv cmd", Send_PGV_Read_CMD, RT_NULL, 60,
                  RT_TIMER_FLAG_PERIODIC | RT_TIMER_FLAG_SOFT_TIMER); /* 60ms定时读取传感器数据 */

    rt_result = rt_thread_init(&location_thread, "location thread", Location_Thread,
                               RT_NULL, location_thread_stack, sizeof(location_thread_stack), 6, 5);
    if (rt_result == RT_EOK)
    {
        rt_thread_startup(&location_thread); /* 启动线程 */
    }
    else
    {
        rt_kprintf("location thread error\n");
    }

    rt_thread_t thread_temp = RT_NULL; /* 线程句柄临时变量 */
    thread_temp = rt_thread_create("wait pgv init", Wait_PGV_Thread, RT_NULL, 1024, 8, 2);
    if (thread_temp != RT_NULL)
    {
        rt_thread_startup(thread_temp); /* 启动线程 */
    }
    else
    {
        rt_kprintf("wait pgv thread error\n");
    }

    thread_temp = rt_thread_create("wait TL740 init", Wait_TL740_Thread, RT_NULL, 1024, 8, 2);
    if (thread_temp != RT_NULL)
    {
        rt_thread_startup(thread_temp); /* 启动线程 */
    }
    else
    {
        rt_kprintf("wait TL740 thread error\n");
    }
}

void Location_Thread(void *parameter)
{
    rt_err_t rt_result;
    rt_uint32_t event_value;

    /* 等待定位所需传感器初始化完成 */
    while (1)
    {
        TL740_Return_Data(RT_WAITING_FOREVER, &z_rate, &z_heading); /* 等待TL740获取到正确数据，并返回 */

        rt_result = rt_event_recv(sensor_event, (1 << Sensor_Init_Namespace::PGV_Init_Event) | (1 << Sensor_Init_Namespace::TL740_Init_Event),
                                  RT_EVENT_FLAG_CLEAR | RT_EVENT_FLAG_AND, 0, &event_value); /* 等待PGV,TL740初始化完毕 */
        if (RT_EOK == rt_result)
        {
            break;
        }
    }

    LOG_I("TL740,PGV初始化完毕."); /* 各传感器初始化完毕 */
    rt_event_delete(sensor_event); /* 删除事件集 */

    encoder_tim.Init((uint16_t)65530, 840); /* 定时器6的时钟是84Mhz，840分频，时基10us，最大计数时间655.3ms */
    encoder_tim.Begin();                    /* 打开定时器 */

    rt_timer_start(&send_pgv_read_cmd_timer); /*  启动定时器，定时发送读取传感器数据 */

    while (1)
    {
        rt_result = TL740_Return_Data(RT_WAITING_FOREVER, &z_rate, &z_heading); /* 等待TL740获取到正确数据，并返回 */
        delta_time_s = encoder_tim.Read() / 100000.0f;                          /* 获取时间间隔 */
        encoder_tim.Write(0);                                                   /* 清空计数 */
        Mecanum_AGV_Class::Read_Velocity(agv_motor_velocity.data);              /* 读取车轮线速度 */

        // LOG_D("v:%.2f,%.2f,%.2f,%.2f", agv_motor_velocity.v1, agv_motor_velocity.v2, agv_motor_velocity.v3, agv_motor_velocity.v4);
        // LOG_D("gyro:%.2f,%.2f", z_rate, z_heading);

        Kalman_Filter_Velocity_Namespace::Kalman_Filter_Velocity(agv_motor_velocity, My_Math_Class::Trans_Rad(z_rate), agv_velocoity, agv_velocity_covariance); /* 使用卡尔曼滤波计算速度 */
        // LOG_D("v:%.2f,%.2f,%.2f", agv_velocoity.v_x, agv_velocoity.v_y, agv_velocoity.w);

        rt_result = Return_PGV_Data(&pgv_data, RT_WAITING_NO); /* 尝试读取PGV数据 */
        if (RT_EOK == rt_result)
        {
            /* 二维码有返回数据 */
            if (Data_Matrix_Tag == pgv_data.target)
            {
                /* 读取到了二维码标签 */
                int x_num, y_num; /* 计算传感器的测量值 */
                Set_Data_Matrix_Flag(true, pgv_data.data.dm_data.tag_control_num);
                PGV_Cal_X_Y(&x_num, &y_num, pgv_data.data.dm_data.tag_control_num);
                pgv_measure.x = pgv_data.data.dm_data.x + x_num * 100.0f;
                pgv_measure.y = pgv_data.data.dm_data.y + y_num * 100.0f;
                if (!stop_by_first_read_data_matrix)
                {
                    stop_by_first_read_data_matrix = true;
                    LOG_I("第一次读取到二维码.");
                    Set_Init_Flag(DM_Initialization_Completed);                                           /* 第一次读取到了二维码 */
                    pgv_measure.theta = Coordinate_Class::Angle_Trans(pgv_data.data.dm_data.angle, 0.0f); /* 此时单位是角度 */
                    pgv_measure.theta = My_Math_Class::Trans_Rad(pgv_measure.theta);                      /* 转换为弧度 */
                    Kalman_Filter_Coor_Namespace::Set_Coor(pgv_measure, true);                            /* 直接修改坐标 */
                    Write_Brake_Event(First_Read_DM);                                                     /* 第一次读取到二维码需修改运动线程中的虚拟坐标，且需要控制AGV抱闸停车 */
                }
                else
                {
                    pgv_measure.theta = Coordinate_Class::Angle_Trans(pgv_data.data.dm_data.angle, My_Math_Class::Trans_Angle(agv_coor.theta)); /* 转换角度到同一周期 */
                    pgv_measure.theta = My_Math_Class::Trans_Rad(pgv_measure.theta);                                                            /* 转换为弧度 */
                    Kalman_Filter_Coor_Namespace::Kalman_Filter_Coor(agv_velocoity, agv_velocity_covariance, pgv_measure, delta_time_s);        /* 使用二维码更新AGV坐标 */
                }
            }
            else
            {
                Set_Data_Matrix_Flag(false, 0);
                Kalman_Filter_Coor_Namespace::Update_Coor_No_Measurement(agv_velocoity, agv_velocity_covariance, delta_time_s); /* 直接计算坐标 */
            }
        }
        else
        {
            Kalman_Filter_Coor_Namespace::Update_Coor_No_Measurement(agv_velocoity, agv_velocity_covariance, delta_time_s); /* 直接计算坐标 */
        }

        rt_enter_critical();
        agv_coor = Kalman_Filter_Coor_Namespace::Return_Coor(); /* 保存计算得到的坐标 */
        location_coor.x_coor = agv_coor.x;
        location_coor.y_coor = agv_coor.y;
        location_coor.theta = agv_coor.theta;
        location_velocity.vx = agv_velocoity.v_x;
        location_velocity.vy = agv_velocoity.v_y;
        location_velocity.w = agv_velocoity.w;
        location_coor.theta = My_Math_Class::Trans_Angle(location_coor.theta);
        location_velocity.w = My_Math_Class::Trans_Angle(location_velocity.w);
        rt_exit_critical();
    }
}

/**
 * @description: 等待PGV传感器初始化完成线程
 * @param {type} 
 * @return: 
 */
void Wait_PGV_Thread(void *parameter)
{
    rt_err_t rt_result;
    rt_thread_delay(500); /* 延时1s，等待PGV传感器自身上电初始化 */
    while (1)
    {
        PGV_Send(Set_Dir_Best); /* 设置色带读取方向 */
        rt_result = Return_PGV_Data(&pgv_data, 500);
        if (RT_EOK == rt_result)
        {
            /* 表示方向设置完成 */
            LOG_I("PGV设置方向成功.");
            break;
        }
        else
        {
            continue;
        }
    }
    rt_thread_delay(200);
    Set_Init_Flag(PGV_Initialization_Completed);                               /* PGV初始化完成 */
    rt_event_send(sensor_event, (1 << Sensor_Init_Namespace::PGV_Init_Event)); /* 发送PGV初始化完成事件 */
}

/**
 * @description: 等待陀螺仪初始化完成归零线程
 * @param {type} 
 * @return: 
 */
void Wait_TL740_Thread(void *parameter)
{
    rt_err_t rt_result;
    while (1)
    {
        rt_result = TL740_Return_Data(RT_WAITING_FOREVER, &z_rate, &z_heading); /* 等待TL740获取到正确数据，并返回 */

        if (RT_EOK == rt_result)
        {
            LOG_I("TL740初始化完成.");
            break;
        }
        else
        {
            continue;
        }
    }
    Set_Init_Flag(TL740_Initialization_Completed);                               /* TL740初始化完成 */
    rt_event_send(sensor_event, (1 << Sensor_Init_Namespace::TL740_Init_Event)); /* 发送PGV初始化完成事件 */
}

void Send_PGV_Read_CMD(void *parameter)
{
    PGV_Send(Read_PGV_Data); /* 读取数据 */
}

void Read_Location_Coor(struct Location_Coor_Struct *coor)
{
    rt_base_t level = rt_hw_interrupt_disable(); /* 关闭全局中断 */
    coor->x_coor = location_coor.x_coor;
    coor->y_coor = location_coor.y_coor;
    coor->theta = location_coor.theta;
    rt_hw_interrupt_enable(level); /* 恢复全局中断 */
}

void Read_Location_Velocity(struct Location_Velocity_Struct *v)
{
    rt_base_t level = rt_hw_interrupt_disable(); /* 关闭全局中断 */
    v->vx = location_velocity.vx;
    v->vy = location_velocity.vy;
    v->w = location_velocity.w;
    rt_hw_interrupt_enable(level); /* 恢复全局中断 */
}
